////////////////////////////////////////////////////////////////////////////////

#ifndef _PHYSICSENGINE_H_
#include "physicsengine.h"
#endif

////////////////////////////////////////////////////////////////////////////////

#ifndef _PHYSICALOBJECT_H_
#include "physicalobject.h"
#endif

#ifndef _MATHFUNCTIONS_H_
#include "mathfunctions.h"
#endif

#ifndef _SHAPE_H_
#include "shape.h"
#endif

#ifndef _INTERSECTOR_H_
#include "intersector.h"
#endif

////////////////////////////////////////////////////////////////////////////////

namespace PE
{
////////////////////////////////////////////////////////////////////////////////


PhysicsEngine::PhysicsEngine()
{
}


////////////////////////////////////////////////////////////////////////////////


PhysicsEngine::~PhysicsEngine()
{
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::AddMovingObject( PhysicalObject& _object )
{
    m_MovingObjects.insert(&_object);
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::AddStableObject( PhysicalObject& _object )
{
    m_StableObjects.insert(&_object);
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::DeleteObject( PhysicalObject& _object )
{
    m_MovingObjects.erase(&_object);
    m_StableObjects.erase(&_object);
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::Update( size_t _dt )
{
    // Process collisions
    ProcessCollisions();

    // Update objects positions
    for (ObjectsIter it = m_MovingObjects.begin(); it != m_MovingObjects.end(); it++)
        UpdatePosition(**it, _dt);
}


////////////////////////////////////////////////////////////////////////////////


Intersector& PhysicsEngine::GetIntersector( const PhysicalObject& _obj1,
                                            const PhysicalObject& _obj2 ) const
{
    const Shape& shape1 = _obj1.GetShape();
    const Shape& shape2 = _obj2.GetShape();

    return shape1.GetIntersector( shape2 );    
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::UpdatePosition( PhysicalObject& _obj, size_t _dt )
{
    // Get object's current position and velocity
    Vector vel = _obj.GetVelocity();
    Point pos = _obj.GetPosition();
    
    // Calculate new position
    Point newPos(pos.x + vel.vx * _dt, pos.y + vel.vy * _dt);
    _obj.SetPosition(newPos);

    // Calculate new speed
    double resistance = _obj.GetResistanceCoefficient();
    if ( resistance )
    {
        double resistanceForce = VectorLength(vel)*resistance;
        _obj.ApplyImpulse(GetDirection(-vel), resistanceForce * _dt);
    }
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::ProcessCollisions()
{
    for ( ObjectsIter objIt1 = m_MovingObjects.begin(); objIt1 != m_MovingObjects.end(); ++objIt1 )
    {
        // First moving object
        PhysicalObject& obj1 = **objIt1;

        // Process collisions with stable objects
        for ( ObjectsIter stableObjIt = m_StableObjects.begin(); stableObjIt != m_StableObjects.end(); ++stableObjIt )
        {
            // Stable object
            PhysicalObject& stableObj= **stableObjIt;

            // Get intersector
            Intersector& intersector = GetIntersector(obj1, stableObj);

            // Check for intersection of two objects
            intersector.Intersect(obj1, stableObj);
            if( intersector.WasIntersection() )
            {
                ApplyImpulse( obj1, intersector.GetNormal() );

                // Inform objects about collision
                obj1.Collision(stableObj);
            }
        } 

        // Process collisions with moving objects
        ObjectsIter objIt2 = objIt1;
        while ( ++objIt2 != m_MovingObjects.end() )
        {
            // Second moving object
            PhysicalObject& obj2 = **objIt2;

            Intersector& intersector = GetIntersector(obj1, obj2);
            
            // Check for intersection of two objects
            intersector.Intersect(obj1, obj2);
            if ( intersector.WasIntersection() )
            {
                ApplyImpulse( obj1, obj2, intersector.GetNormal() );

                // Inform objects about collision
                obj1.Collision(obj2);
            }
        }
    }
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::ApplyImpulse( PhysicalObject& _obj1, PhysicalObject& _obj2, Direction _impulseDir )
{
    // Get impulse value
    double impulse = _obj1.GetMass() * VectorLength( _obj1.GetVelocity() ) +
                     _obj2.GetMass() * VectorLength( _obj2.GetVelocity() );

    // Apply impulses
    _obj1.ApplyImpulse(_impulseDir, impulse/4.0);
    _obj2.ApplyImpulse(_impulseDir, -impulse/4.0);
}


////////////////////////////////////////////////////////////////////////////////


void PhysicsEngine::ApplyImpulse( PhysicalObject& _obj, Direction _impulseDir )
{
    // Get impulse value
    double impulse = _obj.GetMass() * VectorLength( _obj.GetVelocity() );

    // Apply impulse
    _obj.ApplyImpulse(_impulseDir, -impulse);
}


////////////////////////////////////////////////////////////////////////////////

}; // namespace PE

////////////////////////////////////////////////////////////////////////////////
